clear,clc
%第三个代码先把功能进行打包，然后在包内进行修改,并改进雷达扫描方式
%% 1初始化地图和起始点和目标点
load c.mat                                             %导入准备好的地图，从简单的开始
map = c;
start_x = 13;  start_y = 10;  end_x = 2;  end_y = 10;  %设置起始点和目标点坐标
%start_x = 21;  start_y = 2;  end_x = 2;  end_y = 21;  %设置起始点和目标点坐标
map(start_x,start_y)=0.5;   map(end_x,end_y)=0.51;
Drawamap(start_x,start_y,end_x,end_y,map);               %根据起始点、目标点和地图即可绘制初始地图
%% 2从起始点开始向四周四个方向进行扩展探索，并进行综合评价函数Fn的排序，若Fn相同，则选择启发函数最小的进行一步行动
nowlocation = [start_x, start_y];                      %确定当前位置
explore=[];                                            %初始化探索矩阵
path = [];                                             %路径矩阵
anpath = [];                                           %返回时的路径
pic_num = 1;
barrier = map;
step = 0;
while 1
    [true_neighbor,barrier]=goaheade(nowlocation,barrier,end_x,end_y);

    [map,explore,path,nowlocation,barrier]=myAstar(explore,true_neighbor,path,barrier,nowlocation);
    
    Drawapath(start_x,start_y,end_x,end_y,map,explore,path)  %画图，画出目前探索的地图和走过的路径
    step = step+1;
    makeaGIF(pic_num)                                        %制作动图
    pic_num = pic_num + 1;                                   %动图帧数
    if nowlocation(1,1)==end_x && nowlocation(1,2)==end_y    %判断是否到达终点，若到达则跳出循环结束
        break
    end
end

